#include "stm32f10x.h"                
#include "Delay.h"
#include "OLED.h"
#include "MPU6050.h"
#include "nrf24l01.h"  // 添加NRF头文件
#include <math.h>

// 圆周率定义
#ifndef M_PI
#define M_PI 3.1415926535f
#endif

#define FILTER_ALPHA 0.8f        // 一阶低通滤波系数
#define ANGLE_RANGE 30.0f        // MPU6050有效倾角范围
#define LOOP_INTERVAL 8          // 主循环间隔：8ms

// 舵机角度映射范围
#define SERVO1_MIN 30.0f         // 舵机1角度范围
#define SERVO1_MAX 150.0f        
#define SERVO2_MIN 0.0f          // 舵机2角度范围
#define SERVO2_MAX 180.0f        

// NRF配置
u8 txAddr[] = {0x10, 0x20, 0x30, 0x40, 0x53};  // 发送地址
u8 nrfChannel = 90;                             // NRF通道

uint8_t ID;                      // MPU6050设备ID
int16_t AX, AY, AZ;              // 加速度计原始数据
float AX_g, AY_g, AZ_g;          // 转换为重力加速度
float ThetaX, ThetaY;            // 计算得到的倾角
float S1_Angle, S2_Angle;        // 映射后的舵机目标角度
float S1_Filtered, S2_Filtered;  // 滤波后舵机角度

// 自定义舵机角度数据结构 (16字节，适合NRF传输)
struct servo_angle_data {
    uint16_t servo1_angle;  // 舵机1角度 (放大10倍)
    uint16_t servo2_angle;  // 舵机2角度 (放大10倍)
    uint8_t checksum;       // 校验和
    uint8_t reserved[11];   // 保留字节，凑够16字节
};

// 计算校验和
uint8_t calculate_checksum(uint16_t s1, uint16_t s2) {
    return (uint8_t)((s1 + s2) & 0xFF);
}

// NRF发送双舵机角度数据
void NRF_Send_DualAngle() {
    struct servo_angle_data angle_data;
    
    // 角度放大10倍转整数
    angle_data.servo1_angle = (uint16_t)(S1_Filtered * 10);
    angle_data.servo2_angle = (uint16_t)(S2_Filtered * 10);
    angle_data.checksum = calculate_checksum(angle_data.servo1_angle, angle_data.servo2_angle);
    
    // 发送数据
    u8 ret = NRF_Tx_Dat((u8 *)&angle_data, sizeof(angle_data));
    
    // 显示发送状态
    if(ret == TX_DS) {
        OLED_ShowString(0, 0, "NRF TX OK ", OLED_8X16);
    } else if(ret == MAX_RT) {
        OLED_ShowString(0, 0, "NRF MAXRT ", OLED_8X16);
    } else {
        OLED_ShowString(0, 0, "NRF ERROR ", OLED_8X16);
    }
}

int main(void)
{
    // 初始化
    OLED_Init();          
    MPU6050_Init();      
    SPI_NRF_Init();       // 初始化NRF
    
    // 检查NRF连接
    u8 nrf_ret = NRF_Check();
    if (nrf_ret == SUCCESS) {
        OLED_ShowString(10, 0, "NRF OK", OLED_8X16);
    } else {
        OLED_ShowString(10, 0, "NRF ERR", OLED_8X16);
    }
    
    OLED_ShowString(40, 24, "X:", OLED_8X16);   
    OLED_ShowString(40, 48, "Y:", OLED_8X16);      
    OLED_ShowString(75, 0, "ID:", OLED_8X16);
    ID = MPU6050_GetID();
    OLED_ShowHexNum(100, 0, ID, 2, OLED_8X16); 
    OLED_Update();

    // 配置NRF为发送模式
    NRF_TX_Mode(txAddr, nrfChannel);
    Delay_ms(100);

    // 初始数据校准
    MPU6050_GetData(&AX, &AY, &AZ, 0, 0, 0);
    AX_g = (float)AX * 32 / 65535;  
    AY_g = (float)AY * 32 / 65535;
    AZ_g = (float)AZ * 32 / 65535;
    
    // 计算初始角度
    ThetaX = atan(AX_g / AZ_g) * 180 / M_PI;
    ThetaY = atan(AY_g / AZ_g) * 180 / M_PI;
    
    // 倾角映射为舵机角度
    S1_Angle = ((ThetaX + ANGLE_RANGE)/(2*ANGLE_RANGE)) * (SERVO1_MAX - SERVO1_MIN) + SERVO1_MIN;
    S2_Angle = ((ThetaY + ANGLE_RANGE)/(2*ANGLE_RANGE)) * (SERVO2_MAX - SERVO2_MIN) + SERVO2_MIN;
    S1_Filtered = S1_Angle; 
    S2_Filtered = S2_Angle;

    while (1)
    {
        // 读取MPU6050加速度数据
        MPU6050_GetData(&AX, &AY, &AZ, 0, 0, 0);

        // 转换为重力加速度
        AX_g = (float)AX * 32 / 65535;
        AY_g = (float)AY * 32 / 65535;
        AZ_g = (float)AZ * 32 / 65535;

        // 计算倾角（避免除零）
        if (fabs(AZ_g) < 0.1f) AZ_g = (AZ_g > 0) ? 0.1f : -0.1f;
        ThetaX = atan(AX_g / AZ_g) * 180 / M_PI;
        ThetaY = atan(AY_g / AZ_g) * 180 / M_PI;

        // 限制倾角范围
        ThetaX = (ThetaX < -ANGLE_RANGE) ? -ANGLE_RANGE : (ThetaX > ANGLE_RANGE ? ANGLE_RANGE : ThetaX);
        ThetaY = (ThetaY < -ANGLE_RANGE) ? -ANGLE_RANGE : (ThetaY > ANGLE_RANGE ? ANGLE_RANGE : ThetaY);

        // 倾角映射为舵机角度
        S1_Angle = ((ThetaX + ANGLE_RANGE)/(2*ANGLE_RANGE)) * (SERVO1_MAX - SERVO1_MIN) + SERVO1_MIN;
        S2_Angle = ((ThetaY + ANGLE_RANGE)/(2*ANGLE_RANGE)) * (SERVO2_MAX - SERVO2_MIN) + SERVO2_MIN;
        
        // 强制限幅
        S1_Angle = (S1_Angle < SERVO1_MIN) ? SERVO1_MIN : (S1_Angle > SERVO1_MAX ? SERVO1_MAX : S1_Angle);
        S2_Angle = (S2_Angle < SERVO2_MIN) ? SERVO2_MIN : (S2_Angle > SERVO2_MAX ? SERVO2_MAX : S2_Angle);

        // 一阶低通滤波
        S1_Filtered = S1_Angle * FILTER_ALPHA + S1_Filtered * (1 - FILTER_ALPHA);
        S2_Filtered = S2_Angle * FILTER_ALPHA + S2_Filtered * (1 - FILTER_ALPHA);

        // NRF发送舵机角度
        NRF_Send_DualAngle();

        // OLED显示
        static uint8_t showCnt = 0;
        if (showCnt >= 8) {
            OLED_ShowNum(60, 24, (uint16_t)S1_Filtered, 3, OLED_8X16);  // 舵机1角度
            OLED_ShowNum(60, 48, (uint16_t)S2_Filtered, 3, OLED_8X16);  // 舵机2角度
            OLED_Update();
            showCnt = 0;
        } else {
            showCnt++;
        }

        // 主循环延时
        Delay_ms(LOOP_INTERVAL);
    }
}